#include "MapViewer.h"
#include "MapPoint.h"
#include "KeyFrame.h"
#include "LevelHelpers.h"
#include "RandomHelpers.h"
#include "SE3GaussianPDF.h"
#include <iomanip>

#include <cvd/gl_helpers.h>

using namespace CVD;
using namespace std;
using namespace GVars3;

MapViewer::MapViewer(const ATANCamera &c, GLWindow2 &glw):
  mCamera(c), mGLWindow(glw)
{
  mse3ViewerFromWorld = SE3<>::exp(makeVector(0,0,2,0,0,0)) * SE3<>::exp(makeVector(0,0,0,0,0,M_PI));//makeVector(0,0,0,0.8 * M_PI,0,0)

  GV3::Register(mgvnGlobalSE3, "System.GlobalSE3", 0, SILENT);
  GV3::Register(mgvnGlobalSO3, "System.GlobalSO3", 0, SILENT);
  GV3::Register(mgvnDrawGlobal, "MapViewer.DrawGlobal", 0, SILENT);
  mnGlobalSE3 = *mgvnGlobalSE3;
  mnGlobalSO3 = *mgvnGlobalSO3;
  mnDrawGlobal = *mgvnDrawGlobal;
}

void MapViewer::DrawMap(Map *pMap, SE3<> se3CamFromWorld, std::vector<TooN::Vector<3> > &vTest)
{
  mMessageForUser.str(""); // Wipe the user message clean
  
  // Update viewer position according to mouse input:
  {
    pair<Vector<6>, Vector<6> > pv6 = mGLWindow.GetMousePoseUpdate();
    SE3<> se3CamFromMC;
    se3CamFromMC.get_translation() = mse3ViewerFromWorld * mv3MassCenter;
    mse3ViewerFromWorld = SE3<>::exp(pv6.first) * se3CamFromMC * SE3<>::exp(pv6.second) * se3CamFromMC.inverse() * mse3ViewerFromWorld;
  }

  mGLWindow.SetupViewport();
  glClearColor(0,0,0,0);
  glClearDepth(1);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  glEnable(GL_POINT_SMOOTH);
  glEnable(GL_LINE_SMOOTH);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  glColorMask(1,1,1,1);

  //glEnable(GL_DEPTH_TEST);
  DrawGrid();
  DrawCamera(se3CamFromWorld);
  DrawMesh(pMap);
  DrawMapDots(pMap, se3CamFromWorld, vTest);
  DrawKeyFrames(pMap);

  boost::shared_lock< boost::shared_mutex > lockKeyFrames(pMap->mKeyFramesLock); // Lock the keyframes, means local map update cannot occur while we draw...
  mMessageForUser << " Map: " << pMap->msLocalMapPoints.size() << "P, " << pMap->msLocalOptimKeyFrames.size() << "KF";
  lockKeyFrames.unlock();

  //glDisable(GL_DEPTH_TEST);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
}

void MapViewer::DrawMapDots(Map *pMap, SE3<> se3CfromW, std::vector<TooN::Vector<3> > &vTest)
{
  SetupFrustum();
  SetupModelView();
  
  glPointSize(3);
  glBegin(GL_POINTS);
  glColor3f(1,1,1);
  for(int i = 0; i < vTest.size(); ++i)
  {
    glVertex(vTest[i]);
  }
  glEnd();

  int nForMass = 0;
  glColor3f(0,1,1);
  glPointSize(3);
  glBegin(GL_POINTS);
  mv3MassCenter = Zeros;

  boost::shared_lock< boost::shared_mutex > lockMapPoints(pMap->mMapPointsLock);
  if(mnDrawGlobal)
  {
    for(std::vector<boost::shared_ptr<MapPoint> >::iterator it = pMap->mvAllMapPoints.begin(); it != pMap->mvAllMapPoints.end(); ++it)
    {
      if((*it)->bFixed)
      {
        Vector<3> v3Pos = (*it)->v3WorldPos;
        glColor(makeVector(1.0,1.0,1.0));
        glVertex(v3Pos);
      }
      else if(!(*it)->bBad && 10*(*it)->nMEstimatorOutlierCount < (*it)->nMEstimatorInlierCount && (*it)->nMEstimatorInlierCount > 20)
      {
        Vector<3> v3Pos = (*it)->v3WorldPos;
        glColor(gavLevelColors[(*it)->nSourceLevel]);
        //if(se3CfromW.get_translation() * v3Pos < 10000)
        if(v3Pos * v3Pos < 10000)
        {
          nForMass++;
          mv3MassCenter += v3Pos;
        }
        glVertex(v3Pos);
      }
    }
  }
  else
  {
    for(std::set<boost::shared_ptr<MapPoint> >::iterator it = pMap->msLocalMapPoints.begin(); it != pMap->msLocalMapPoints.end(); ++it)
    {
      if((*it)->bFixed)
      {
        Vector<3> v3Pos = (*it)->v3WorldPos;
        glColor(makeVector(1.0,1.0,1.0));
        glVertex(v3Pos);
      }
      else if(!(*it)->bBad && 10*(*it)->nMEstimatorOutlierCount < (*it)->nMEstimatorInlierCount && (*it)->nMEstimatorInlierCount > 20)
      {
        Vector<3> v3Pos = (*it)->v3WorldPos;
        glColor(gavLevelColors[(*it)->nSourceLevel]);
        //if(se3CfromW.get_translation() * v3Pos < 10000)
        if(v3Pos * v3Pos < 10000)
        {
          nForMass++;
          mv3MassCenter += v3Pos;
        }
        glVertex(v3Pos);
      }
    }
  }

  lockMapPoints.unlock();
  glEnd();
  mv3MassCenter = mv3MassCenter / (0.1 + nForMass);
}


void MapViewer::DrawGrid()
{
  SetupFrustum();
  SetupModelView();
  glLineWidth(1);
  
  glBegin(GL_LINES);
  
  // Draw a larger grid around the outside..
  double dGridInterval = 0.1;
  
  double dMin = -100.0 * dGridInterval;
  double dMax =  100.0 * dGridInterval;
  
  for(int x=-10;x<=10;x+=1)
    {
      if(x==0)
	glColor3f(1,1,1);
      else
	glColor3f(0.3,0.3,0.3);
      glVertex3d((double)x * 10 * dGridInterval, dMin, 0.0);
      glVertex3d((double)x * 10 * dGridInterval, dMax, 0.0);
    }
  for(int y=-10;y<=10;y+=1)
    {
      if(y==0)
	glColor3f(1,1,1);
      else
	glColor3f(0.3,0.3,0.3);
      glVertex3d(dMin, (double)y * 10 *  dGridInterval, 0.0);
      glVertex3d(dMax, (double)y * 10 * dGridInterval, 0.0);
    }
  
  glEnd();

  glBegin(GL_LINES);
  dMin = -10.0 * dGridInterval;
  dMax =  10.0 * dGridInterval;
  
  for(int x=-10;x<=10;x++)
    {
      if(x==0)
	glColor3f(1,1,1);
      else
	glColor3f(0.5,0.5,0.5);
      
      glVertex3d((double)x * dGridInterval, dMin, 0.0);
      glVertex3d((double)x * dGridInterval, dMax, 0.0);
    }
  for(int y=-10;y<=10;y++)
  {
    if(y==0)
      glColor3f(1,1,1);
    else
      glColor3f(0.5,0.5,0.5);
    glVertex3d(dMin, (double)y * dGridInterval, 0.0);
    glVertex3d(dMax, (double)y * dGridInterval, 0.0);
  }
  
  glColor3f(1,0,0);
  glVertex3d(0,0,0);
  glVertex3d(1,0,0);
  glColor3f(0,1,0);
  glVertex3d(0,0,0);
  glVertex3d(0,1,0);
  glColor3f(1,1,1);
  glVertex3d(0,0,0);
  glVertex3d(0,0,1);
  glEnd();
}


void MapViewer::DrawMesh(Map *pMap)
{
  glEnable(GL_LINE_SMOOTH);
  glEnable(GL_BLEND);
  glEnable(GL_TEXTURE_2D);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  glLineWidth(2);

  glTexEnvf( GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE );

  // the texture is clamped at the edges
  glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP );
  glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP );

  boost::shared_lock< boost::shared_mutex > lockMesh(pMap->mMeshLock);

  std::vector<boost::shared_ptr<Cube> > cubes;
  unsigned int cube_N = pMap->mesh.GetAllCubes(cubes);//, mv6BoundsPVS[0], mv6BoundsPVS[1], mv6BoundsPVS[2], mv6BoundsPVS[3], mv6BoundsPVS[4], mv6BoundsPVS[5]);
  Vector<2> v2ImageSize = mCamera.GetImageSize();

  for(unsigned int i = 0; i < cube_N; ++i)
  {
    for(unsigned int j = 0; j < cubes[i]->faces.size(); ++j)
    {
      glBindTexture(GL_TEXTURE_2D, cubes[i]->faces[j]->patches[0]->gluTextureId);
      glBegin(GL_TRIANGLES);
      glTexCoord2d(cubes[i]->faces[j]->patches[0]->x1/v2ImageSize[0], cubes[i]->faces[j]->patches[0]->y1/v2ImageSize[1]); 
      glVertex3d(cubes[i]->faces[j]->x1, cubes[i]->faces[j]->y1, cubes[i]->faces[j]->z1);
      glTexCoord2d(cubes[i]->faces[j]->patches[0]->x2/v2ImageSize[0], cubes[i]->faces[j]->patches[0]->y2/v2ImageSize[1]); 
      glVertex3d(cubes[i]->faces[j]->x2, cubes[i]->faces[j]->y2, cubes[i]->faces[j]->z2);
      glTexCoord2d(cubes[i]->faces[j]->patches[0]->x3/v2ImageSize[0], cubes[i]->faces[j]->patches[0]->y3/v2ImageSize[1]); 
      glVertex3d(cubes[i]->faces[j]->x3, cubes[i]->faces[j]->y3, cubes[i]->faces[j]->z3);
      glEnd();

      /*glColor4f(.0, 0.5, .0, 0.6);
      glBegin(GL_LINE_STRIP);
      glVertex3d(cubes[i]->faces[j]->x1, cubes[i]->faces[j]->y1, cubes[i]->faces[j]->z1);
      glVertex3d(cubes[i]->faces[j]->x2, cubes[i]->faces[j]->y2, cubes[i]->faces[j]->z2);
      glVertex3d(cubes[i]->faces[j]->x3, cubes[i]->faces[j]->y3, cubes[i]->faces[j]->z3);
      glEnd();*/
    }
  }
  lockMesh.unlock();
  glLineWidth(1);
  glColor3f(1,0,0);
  glDisable(GL_TEXTURE_2D);
}

string MapViewer::GetMessageForUser()
{
  return mMessageForUser.str();
}

void MapViewer::SetupFrustum()
{
  glMatrixMode(GL_PROJECTION);  
  glLoadIdentity();
  double zNear = 0.03;
  glFrustum(-zNear, zNear, 0.75*zNear,-0.75*zNear,zNear,50);
  glScalef(1,1,-1);
  return;
};

void MapViewer::SetupModelView(SE3<> se3WorldFromCurrent)
{
  glMatrixMode(GL_MODELVIEW);  
  glLoadIdentity();
  glMultMatrix(mse3ViewerFromWorld * se3WorldFromCurrent);
  return;
};

void MapViewer::DrawCamera(SE3<> se3CfromW)
{
  SetupModelView(se3CfromW.inverse());
  SetupFrustum();
  
  glLineWidth(3);
  glBegin(GL_LINES);
  glColor3f(1,0,0);
  glVertex3f(0.0f, 0.0f, 0.0f);
  glVertex3f(0.1f, 0.0f, 0.0f);
  glColor3f(0,1,0);
  glVertex3f(0.0f, 0.0f, 0.0f);
  glVertex3f(0.0f, 0.1f, 0.0f);
  glColor3f(1,1,1);
  glVertex3f(0.0f, 0.0f, 0.0f);
  glVertex3f(0.0f, 0.0f, 0.1f);
  glEnd();
}

void MapViewer::DrawKeyFrames(Map *pMap)
{
  boost::shared_lock< boost::shared_mutex > lockKeyFrames(pMap->mKeyFramesLock); // Lock the keyframes, means local map update cannot occur while we draw...
  if(mnDrawGlobal)
  {
    RTree<KeyFrame, double, 3>::Iterator itree;
    pMap->mrtAllKeyFrames.GetFirst(itree);

    while(!pMap->mrtAllKeyFrames.IsNull(itree))
    {
      boost::shared_ptr<KeyFrame> kf = pMap->mrtAllKeyFrames.GetAt(itree);
      DrawKeyFrame(pMap, kf);
      pMap->mrtAllKeyFrames.GetNext(itree);
    }
  }
  else
  {
    for(std::set<boost::shared_ptr<KeyFrame> >::iterator it = pMap->msLocalOptimKeyFrames.begin(); it != pMap->msLocalOptimKeyFrames.end(); ++it)
      DrawKeyFrame(pMap, *it);
  }  

  lockKeyFrames.unlock();
}


void MapViewer::DrawKeyFrame(Map *pMap, boost::shared_ptr<KeyFrame> kf)
{
  bool bDrawGlobal = false;
  SE3<> se3WfromC = kf->se3CfromW.inverse();
  SE3GaussianPDF se3PdfWfromC = kf->ComputeGlobalWfromC(pMap->GetFfromW());
  const TooN::Matrix<6,6>& m66CovWfromC = se3PdfWfromC.get_covariance();

  SetupModelView(se3WfromC);
  SetupFrustum();
  
  //Vector<3> v3Test = (se3WfromC * se3PdfWfromC.get_mean().inverse()).get_translation();
  //Vector<3> v3Test = (se3WfromC.inverse() * se3PdfWfromC.get_mean()).get_translation();

  glLineWidth(1);
  glBegin(GL_LINES);
  glColor3f(1,0,0);
  glVertex3f(0.0f, 0.0f, 0.0f);
  glVertex3f(0.1f, 0.0f, 0.0f);
  glColor3f(0,1,0);
  glVertex3f(0.0f, 0.0f, 0.0f);
  glVertex3f(0.0f, 0.1f, 0.0f);
  glColor3f(1,1,1);
  glVertex3f(0.0f, 0.0f, 0.0f);
  glVertex3f(0.0f, 0.0f, 0.1f);

  //glColor3f(1,1,1);
  //glVertex3f(0.0f, 0.0f, 0.0f);
  //glVertex3f(v3Test[0],v3Test[1],v3Test[2]);

  glEnd();

  if(kf->mse3PdfFfromC.has_rotation())//(mnGlobalSO3 || mnGlobalSE3) && 
  {
    bDrawGlobal = true;

    if(!mnGlobalSE3 || !kf->mse3PdfFfromC.has_translation())
      se3PdfWfromC.get_translation() = se3WfromC.get_translation();
  }

  if(bDrawGlobal)
  {
    const double alpha = 0.2;
    double dRoll2Sigma  = 2.0*m66CovWfromC(3,3)*RAD_2_DEG;
    double dPitch2Sigma = 2.0*m66CovWfromC(4,4)*RAD_2_DEG;
    double dYaw2Sigma   = 2.0*m66CovWfromC(5,5)*RAD_2_DEG;

    glEnable(GL_BLEND);
    glEnable(GL_POINT_SMOOTH);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  
    /*SE3<> se3TestWfromC = pMap->GetFfromW().inverse() * pMap->GetGfromF().get_mean().inverse() * kf->mse3PdfGfromC.get_mean();
    se3TestWfromC.get_translation() = se3WfromC.get_translation();
    SetupModelView(se3TestWfromC);
    glLineWidth(1);
    glBegin(GL_LINES);
    glColor4f(1,0,1,1);
    glVertex3f(0.0f, 0.0f, 0.0f);
    glVertex3f(0.1f, 0.0f, 0.0f);
    glColor4f(1,0,1,1);
    glVertex3f(0.0f, 0.0f, 0.0f);
    glVertex3f(0.0f, 0.1f, 0.0f);
    glColor4f(1,0,1,1);
    glVertex3f(0.0f, 0.0f, 0.0f);
    glVertex3f(0.0f, 0.0f, 0.1f);
    glEnd();*/

    // Drawing in XY plane...
    /*SetupModelView(se3PdfWfromC.get_mean());

    glLineWidth(1);
    glBegin(GL_LINES);
    glColor4f(1,0,0,0.5);
    glVertex3f(0.0f, 0.0f, 0.0f);
    glVertex3f(0.1f, 0.0f, 0.0f);
    glColor4f(0,1,0,0.5);
    glVertex3f(0.0f, 0.0f, 0.0f);
    glVertex3f(0.0f, 0.1f, 0.0f);
    glColor4f(1,1,1,0.5);
    glVertex3f(0.0f, 0.0f, 0.0f);
    glVertex3f(0.0f, 0.0f, 0.1f);
    glEnd();

    glColor4f(0,1,0,alpha);// Y-axis is green
    GLUquadricObj* quadAxisYawY = gluNewQuadric();
    gluPartialDisk(quadAxisYawY, 0.0, 0.1, 10, 10, -dYaw2Sigma, 2.0*dYaw2Sigma);
    gluDeleteQuadric(quadAxisYawY);

    glColor4f(1,0,0,alpha);// X-axis is red
    GLUquadricObj* quadAxisYawX = gluNewQuadric();
    gluPartialDisk(quadAxisYawX, 0.0, 0.1, 10, 10, 90.0-dYaw2Sigma, 2.0*dYaw2Sigma);
    gluDeleteQuadric(quadAxisYawX);

    // Drawing in YZ plane...
    SE3<> se3WfromC_YZ = se3PdfWfromC.get_mean();
    se3WfromC_YZ.get_rotation() *= PackSO3_RPY(0.0, -M_PI/2, 0.0);
    SetupModelView(se3WfromC_YZ);
    glColor4f(0,1,0,alpha);// Y-axis is green
    GLUquadricObj* quadAxisRollY = gluNewQuadric();
    gluPartialDisk(quadAxisRollY, 0.0, 0.1, 10, 10, -dRoll2Sigma, 2.0*dRoll2Sigma);
    gluDeleteQuadric(quadAxisRollY);

    glColor4f(1,1,1,alpha);// Z-axis is white
    GLUquadricObj* quadAxisRollZ = gluNewQuadric();
    gluPartialDisk(quadAxisRollZ, 0.0, 0.1, 10, 10, 90.0-dRoll2Sigma, 2.0*dRoll2Sigma);
    gluDeleteQuadric(quadAxisRollZ);

    // Drawing in XZ plane...
    SE3<> se3WfromC_XZ = se3PdfWfromC.get_mean();
    se3WfromC_XZ.get_rotation() *= PackSO3_RPY(M_PI/2, 0.0, 0.0);
    SetupModelView(se3WfromC_XZ);
    glColor4f(1,1,1,alpha);// Z-axis is white
    GLUquadricObj* quadAxisPitchZ = gluNewQuadric();
    gluPartialDisk(quadAxisPitchZ, 0.0, 0.1, 10, 10, -dPitch2Sigma, 2.0*dPitch2Sigma);
    gluDeleteQuadric(quadAxisPitchZ);

    glColor4f(1,0,0,alpha);// X-axis is red
    GLUquadricObj* quadAxisPitchX = gluNewQuadric();
    gluPartialDisk(quadAxisPitchX, 0.0, 0.1, 10, 10, 90.0-dPitch2Sigma, 2.0*dPitch2Sigma);
    gluDeleteQuadric(quadAxisPitchX);*/

    if(se3PdfWfromC.has_translation())
    {
      //std::cout << m66CovWfromC << std::endl << std::endl;

      SetupModelView(SE3<>(SO3<>(), se3PdfWfromC.get_translation()));
      GLUquadricObj* quadPositionEllipse = gluNewQuadric();
      glColor4f(0,1,1,0.05);
      glScalef(0.01*m66CovWfromC(0,0), 0.01*m66CovWfromC(1,1), 0.01*m66CovWfromC(2,2));
      gluSphere(quadPositionEllipse, 1.0, 10, 10);
      gluDeleteQuadric(quadPositionEllipse);
    }
    glDisable(GL_POINT_SMOOTH);
    glDisable(GL_BLEND);
  }
}


